// --------------------------------------------------------------------------------------------------------------------
// <copyright file="LoopClosingAgent.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Microsoft.Robotics.Navigation.Runtime.Localization
{
    using System;
    using System.Collections.Generic;
    using System.Runtime.Serialization;
    using System.Timers;

    using Microsoft.Robotics.Interaction.Runtime.Input;
    using Microsoft.Robotics.Navigation.Perception;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Runtime;
    
    /// <summary>
    /// Message that contains loop closure information
    /// </summary>
    [DataContract]
    public class LoopClosingAgent : SynchronizedConsumerProducerAgent<HDPwithPoseMessage<KinectHorizontalDepthProfile>, GamePadAgentMessage, LoopClosingMessage>,
        ISubscriptionReceiver<CompletionMessage>
    {
        /// <summary>
        /// File of the absolute poses at the markers
        /// </summary>
        [DataMember]
        private string groundTruthAbsoluteFile;

        /// <summary>
        /// File of the ground truth data at left shoulder hit
        /// </summary>
        [DataMember]
        private string groundTruthFile;

        /// <summary>
        /// slam producer
        /// </summary>
        [DataMember]
        private string slamProducer;

        /// <summary>
        /// gamepad producer
        /// </summary>
        [DataMember]
        private string gamePadProducer;

        /// <summary>
        /// completion message producer name
        /// </summary>
        [DataMember]
        private string completionMessageProducer;

        /// <summary>
        /// The expected number of Y buttons
        /// </summary>
        [DataMember]
        private int expectedYbuttons;

        /// <summary>
        /// Flag indicating if the Y button is down
        /// </summary>
        private bool ybuttonDown;

        /// <summary>
        /// Counter that counts the number of Y buttons pressed
        /// </summary>
        private int ybuttonCounter;

        /// <summary>
        /// completion message producer agent
        /// </summary>
        private IAgentHost completionMessageAgent;

        /// <summary>
        /// List of indices where the markers are located
        /// </summary>
        private List<int> markerIndices;

        /// <summary>
        /// List of absolute poses of the robot under markers
        /// </summary>
        private List<Pose2D> absolutePoses;

        /// <summary>
        /// left shoulder down
        /// </summary>
        private bool leftShoulderDown;

        /// <summary>
        /// the number of left shoulder buttons pressed
        /// </summary>
        private int leftShoulderCounter;

        /// <summary>
        /// The expected number of left shoulders to be pressed
        /// </summary>
        private int expectedLeftShoulders;

        /// <summary>
        /// Initializes a new instance of the LoopClosingAgent class
        /// </summary>
        /// <param name="name">name of the agent</param>
        /// <param name="slamProducer">Producer of slam messages</param>
        /// <param name="gamePadProducer">game pad message producer</param>
        /// <param name="completionMessageProducer">completion message producer</param>
        /// <param name="groundTruthFile">Used to count the number of left shoulders hit in the run</param>
        /// <param name="groundTruthAbsoluteFile">File of $GroundTruthAbsolute.txt$</param>
        /// <param name="expectedYbuttons">The number of times the Y button is expected to be pressed</param>
        public LoopClosingAgent(
            string name,
            IProducer<HDPwithPoseMessage<KinectHorizontalDepthProfile>> slamProducer,
            IProducer<GamePadAgentMessage> gamePadProducer,
            IProducer<CompletionMessage> completionMessageProducer,
            string groundTruthFile,
            string groundTruthAbsoluteFile = null,
            int expectedYbuttons = 0) :
            base(name, 100)
        {
            this.groundTruthFile = groundTruthFile;
            this.groundTruthAbsoluteFile = groundTruthAbsoluteFile;
            this.slamProducer = slamProducer.Name;
            this.gamePadProducer = gamePadProducer.Name;
            this.completionMessageProducer = completionMessageProducer.Name;
            this.expectedYbuttons = expectedYbuttons;
        }

        /// <summary>
        /// Gets the name of the agent that produces local maps.
        /// </summary>
        protected override string ProducerName1
        {
            get { return this.slamProducer; }
        }

        /// <summary>
        /// Gets the name of the agent that generates odometry info.
        /// </summary>
        protected override string ProducerName2
        {
            get { return this.gamePadProducer; }
        }

        /// <summary>
        /// Called to initialize the agent
        /// </summary>
        /// <param name="locator">agent locator</param>
        public override void Initialize(AgentLocator locator)
        {
            base.Initialize(locator);
            this.completionMessageAgent = locator.GetAgent(this.completionMessageProducer);
        }

        /// <summary>
        /// Called on activation of the agent
        /// </summary>
        public override void OnActivated()
        {
            base.OnActivated();
            this.markerIndices = new List<int>();
            this.absolutePoses = new List<Pose2D>();
            this.leftShoulderDown = false;
            this.leftShoulderCounter = 0;
            this.ybuttonDown = false;
            this.ybuttonCounter = 0;

            // Use left shoulder buttons to send loops closures
            if (this.expectedYbuttons == 0)
            {
                System.IO.StreamReader groundTruthReader = new System.IO.StreamReader(this.groundTruthFile);
                this.expectedLeftShoulders = 0;
                while (groundTruthReader.EndOfStream == false)
                {
                    string data = groundTruthReader.ReadLine();
                    if (data[0] != '#')
                    {
                        this.expectedLeftShoulders++;
                    }
                }

                this.expectedLeftShoulders++;
                groundTruthReader.Close();
                if (this.groundTruthAbsoluteFile != null)
                {
                    System.IO.StreamReader reader = new System.IO.StreamReader(this.groundTruthAbsoluteFile);
                    while (reader.EndOfStream == false)
                    {
                        string data = reader.ReadLine();
                        string[] parts = data.Split(' ');
                        this.absolutePoses.Add(new Pose2D(Convert.ToDouble(parts[1]), Convert.ToDouble(parts[2]), Convert.ToDouble(parts[3])));
                    }

                    reader.Close();
                }
            }
            else
            {
                // Use Y button to send loop closure
                for (int i = 0; i < this.expectedYbuttons + 1; i++)
                {
                    this.absolutePoses.Add(new Pose2D(0, 0, 0));
                }
            }
        }

        /// <summary>
        /// Called when a completion message is received
        /// </summary>
        /// <param name="message">The completion message</param>
        public void Receive(CompletionMessage message)
        {
            // Never get a completion message from the gamepad
            Console.WriteLine("Nevermind, it does");
        }

        /// <summary>
        /// Called when a gamepad message is received
        /// </summary>
        /// <param name="message">The message</param>
        /// <param name="gamepadMessage">gamepad message</param>
        protected override void Receive(HDPwithPoseMessage<KinectHorizontalDepthProfile> message, GamePadAgentMessage gamepadMessage)
        {
            if (this.expectedYbuttons == 0)
            {
                if (gamepadMessage.Buttons.LeftShoulder && this.leftShoulderDown == false)
                {
                    if (this.groundTruthFile != null)
                    {
                        this.markerIndices.Add(message.FrameIndex);
                    }

                    this.leftShoulderDown = true;
                    this.leftShoulderCounter++;
                }

                if (gamepadMessage.Buttons.LeftShoulder == false)
                {
                    this.leftShoulderDown = false;
                }

                if (this.leftShoulderCounter == this.expectedLeftShoulders)
                {
                    this.SendLoopClosures(message.FrameIndex);
                    this.leftShoulderCounter++;
                }
            }
            else
            {
                // Assume first slam message is where we start/stop
                if (this.markerIndices.Count == 0)
                {
                    this.markerIndices.Add(message.FrameIndex);
                }

                // Y button pressed
                if (this.ybuttonDown == false && gamepadMessage.Buttons.Y)
                {
                    this.markerIndices.Add(message.FrameIndex);
                    this.ybuttonCounter++;
                    this.ybuttonDown = true;
                }

                // avoid multiple resends of the Y button
                if (!gamepadMessage.Buttons.Y)
                {
                    this.ybuttonDown = false;
                }

                // Number of expected Y buttons has been reached, tell Graph to optimize
                if (this.expectedYbuttons == this.ybuttonCounter)
                {
                    this.SendLoopClosures(message.FrameIndex);
                    this.ybuttonCounter++;
                }
            } 
        }

        /// <summary>
        /// Publishes all the loop closing messages
        /// </summary>
        /// <param name="frameIndex">The index of the node to close</param>
        private void SendLoopClosures(int frameIndex)
        {
            if (this.expectedYbuttons > 0 || this.groundTruthAbsoluteFile != null)
            {
                if (this.markerIndices.Count > 0)
                {
                    for (int i = 0; i < this.markerIndices.Count - 1; i++)
                    {
                        for (int j = i + 1; j < this.markerIndices.Count; j++)
                        {
                            CoordinateTransform2D transform = new CoordinateTransform2D(this.absolutePoses[i]).Reverse();
                            this.Publisher.Post(new LoopClosingMessage(this.markerIndices[i], this.markerIndices[j], transform.Transform(this.absolutePoses[j]), false), true);
                        }
                    }
                }

                this.Publisher.Post(new LoopClosingMessage(), true);
            }
            else
            {
                this.Publisher.Post(new LoopClosingMessage(0, frameIndex - 1, new Pose2D(), true), true);
            }
        }
    }
}
